#pragma once
#include "rtdev.h"
#include <vector>
#include <map>
#include <mutex>

#include "robot_status_common.h"

namespace rtdev {

class ServoItf;
class RtRobot
{
public:
    static const size_t ROBOT_MAX_JOINT_NUM = 7;
    static const int ROBOT_DH_ENABLED_FLAG = 0x5555;
    static const int ROBOT_DH_VERSION = 1;
    static const int ROBOT_DYN_ENABLED_FLAG = 0x5555;
    enum RobotType
    {
        ROBOT_TYPE_ZU = 0,
        ROBOT_TYPE_MC = 1,
        ROBOT_TYPE_AT = 2,
        ROBOT_TYPE_MAX = 3,
    };
    enum DHDYNReadStatus
    {
        DHYN_IN_READING = 0,
        DHYN_NOT_USED = 1,
        DHYN_READ_OVER = 2,
        DHYN_IN_READY = 3,
        DHYN_READ_ERR = 4,
    };
    struct DHParam
    {
        DHDYNReadStatus state;  // 0：初始状态，1：参数未启用；2：参数已就绪（完成查询与验证）；3：数据校验错误
        uint32_t enable;
        uint32_t crc;
        int version;
        struct DHParamJoint
        {
            int a;
            int alpha;
            int d;
            int theta;
            int base_offset_1;  // 一级偏移
            int base_offset_2;  // 二级偏移
        } joint[ROBOT_MAX_JOINT_NUM];
    };
    struct DynParam
    {
        DHDYNReadStatus state;  // 0：初始状态，1：参数未启用；2：参数已就绪（完成查询与验证）；3：数据校验错误
        uint8_t ready_num;
        uint32_t enable;
        uint32_t crc;
        float data[66];  // version(1-6), dynP0, dynP1, ..., dynP59
    };
    struct RobotSafetyParam
    {
        uint8_t clsn_sensitivity;
    };
    struct ActiveStatus
    {
        bool is_alive = false;   // 有轴在线
        bool is_active = false;  // 所有轴都在线
    };

    struct RtRobotStatus
    {
        uint32_t serial_num_ = -1;  // 仅由motion线程操作
        DHParam dh;                 // 仅由motion线程操作
        DynParam dyn;               // 仅由motion线程操作
        RobotSafetyParam robot_safety_para;
        ActiveStatus active_status;

        JointParmRsp joint_para;                              // 关节查询数据
        JointPeriodStatus joint_status[ROBOT_MAX_JOINT_NUM];  // 关节周期伺服数据
    };

public:
    RtRobot(int i);
    virtual ~RtRobot();
    virtual void init();

    void link_servo(std::shared_ptr<rtdev::ServoItf>& servo, int i);

    void check_alive();
    virtual void update();
    void reset() { local_rtrobot_status_.serial_num_ = -1; }
    void do_robot_query_setting_when_active();
    void reset_servo();
    size_t joint_num() const { return joint_servo_.size(); }

private:
    RobotType robot_type_ = ROBOT_TYPE_ZU;
    std::vector<std::shared_ptr<rtdev::ServoItf>> joint_servo_;
    int robot_id_ = -2;

private:
    RtRobotStatus local_rtrobot_status_ = {};  // 只给rtdev操作
    // 共享给motion的状态
    std::mutex status_mutex_;
    RtRobotStatus rtrobot_status_fdb_ = {};

public:
    void prepare_status_for_motion();
    void get_rtrobot_status_for_motion(RtRobotStatus* status);

public:
    int query_robot_serial_num();

    int sync_robot_position(const int* pos, int interval_us);
    int sync_robot_torque(const int* torque, const int* grvcomp);
    int set_robot_torque_forward_enable(uint8_t enable);
    void set_robot_series_joint_cfg(uint8_t series, const int32_t* offset, const int* dir);

    int set_robot_joint_collision_sensitivity(uint8_t sensitivity);
    int set_robot_joint_friction_compensate(uint8_t joint, int friciotn);
    int set_robot_joint_brake_lease(uint8_t joint, int relese);
    int set_robot_joint_errpos(uint8_t joint, double errpos);
    int set_robot_joint_min_limit(uint8_t joint, double joint_min_limit);
    int set_robot_joint_max_limit(uint8_t joint, double joint_max_limit);
    int set_robot_joint_multi_turn_step(uint8_t joint, int step);
    int set_robot_joint_avoid_pos_mutation(uint8_t joint, unsigned int para);

    int query_robot_joint_collision_setting();
    int query_robot_joint_motor_supplier_id();

public:
    int query_dh_param();
    int query_dyn_param();
    DHParam& get_dh() { return local_rtrobot_status_.dh; }
    DynParam get_dyn() { return local_rtrobot_status_.dyn; }
    uint32_t get_robot_serial_num() { return local_rtrobot_status_.serial_num_; }

private:
    void update_robot_type();
    void check_dyn_param();

    static const uint32_t PERIOD_QUERY_ALL_INTERAL = 125;
    static const uint32_t PERIOD_QUERY_INTERAL = 5;
    uint32_t period_query_count_ = 0;
    bool period_query_enable_ = false;
    void period_query_joint();

    // 周期数据查询
    int query_robot_joint_temperature();
    int query_robot_joint_average_power();
    int query_robot_joint_current_fluctuation();
    int query_robot_joint_sum_running_circles();
    int query_robot_joint_sum_running_time();
    int query_robot_joint_cur_running_circles();
    int query_robot_joint_cur_running_time();
    int query_robot_joint_voltage();

    // 上电后需要的操作
    int query_robot_joint_version();
    int query_robot_joint_first_poweron_date();
    int set_robot_joint_torque_command_unit(int unit);

    static const uint32_t SERVO_OBJID_SERVO_VERSION = 9007;
    static const uint32_t SERVO_OBJID_POWER_ON_DATE = 9013;
    static const uint32_t SERVO_OBJID_SEROHOME = 2215;
    static const uint32_t SERVO_OBJID_CAB_TYPE = 9011;
    static const uint32_t SERVO_OBJID_JOINT_MIN_LIMIT = 2233;
    static const uint32_t SERVO_OBJID_JOINT_MAX_LIMIT = 2232;
    static const uint32_t SERVO_OBJID_JOINT_FRICTION = 2222;
    static const uint32_t SERVO_OBJID_BREAK_RELEASE = 2240;
    static const uint32_t SERVO_OBJID_MULYI_TURN_STEP = 2241;
    static const uint32_t SERVO_OBJID_TORQUE_UNIT = 9901;
    static const uint32_t SERVO_OBJID_COLLISION_BASIC = 2245;
    static const uint32_t SERVO_OBJID_COLLISION_TH = 2246;
    static const uint32_t SERVO_OBJID_JOINT_WARNING_DEVIATION = 2253;
    static const uint32_t SERVO_OBJID_SERIAL_NUM = 9009;
    static const uint32_t SERVO_OBJID_SUPPLIER_ID = 2000;
    static const uint32_t SERVO_OBJID_TEMPERATURE = 9108;
    static const uint32_t SERVO_OBJID_AVERRAGE_POWER = 9101;
    static const uint32_t SERVO_OBJID_CURRENT_FLUCTUAION = 9102;
    static const uint32_t SERVO_OBJID_SUM_RUNNING_CYCLE = 9103;
    static const uint32_t SERVO_OBJID_SUM_RUNNING_TIME = 9104;
    static const uint32_t SERVO_OBJID_CUR_RUNNING_CYCLE = 9105;
    static const uint32_t SERVO_OBJID_CUR_RUNNING_TIME = 9106;
    static const uint32_t SERVO_OBJID_VOLTAGE = 9107;
    static const uint32_t SERVO_OBJID_BACKDRIVE = 1017;
    static const uint32_t SERVO_OBJID_POSMUTATION = 1018;
};

class RtRobotMgr
{
public:
    static RtRobotMgr& instance()
    {
        static RtRobotMgr instance;
        return instance;
    }
    int add_rtrobot(std::shared_ptr<RtRobot> group);
    int del_rtrobot(uint8_t group_id);
    std::shared_ptr<RtRobot> get_rtrobot(uint8_t group_id);
    void reset();
    size_t robot_num();

private:
    std::map<uint8_t, std::shared_ptr<RtRobot>> rtrobots_;
};

}  // namespace rtdev
